#include <btl/extra/VideoSource/VideoSourceV4L2.hpp>
#include <iostream>
#include <cassert>

namespace btl
{
namespace extra
{
namespace videosource
{

VideoSourceV4L2::VideoSourceV4L2(const char* device, int requestedNumBuffers) :
   _fd(-1)
{
	/*
   _fd = open(device, O_RDWR | O_NONBLOCK);
   if(_fd < 0)
   {
      std::stringstream ss;
      ss << "Could not open \"" << device << "\".";
      throw VideoSourceV4L2::Exception(ss.str());
   }
   
   if(ioctl(_fd, VIDIOC_QUERYCAP, &_cap) < 0)
   {
      std::stringstream ss;
      ss << device << "is not a video4linux device.";
      close(_fd);
      throw VideoSourceV4L2::Exception(ss.str());
   }
   std::cout << "driver: " << _cap.driver << std::endl;
   std::cout << "card: " << _cap.card << std::endl;
   std::cout << "bus_info: " << _cap.bus_info << std::endl;
   std::cout << "version: " << _cap.version  << std::endl;
   std::cout << "capabilities: 0x" << std::hex << _cap.capabilities << std::dec << std::endl;
   std::cout << "V4L2_CAP_VIDEO_CAPTURE        = " << !!(_cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) << std::endl;
   std::cout << "V4L2_CAP_VIDEO_OUTPUT         = " << !!(_cap.capabilities & V4L2_CAP_VIDEO_OUTPUT) << std::endl;
   std::cout << "V4L2_CAP_VIDEO_OVERLAY        = " << !!(_cap.capabilities & V4L2_CAP_VIDEO_OVERLAY) << std::endl;
   std::cout << "V4L2_CAP_VBI_CAPTURE          = " << !!(_cap.capabilities & V4L2_CAP_VBI_CAPTURE) << std::endl;
   std::cout << "V4L2_CAP_VBI_OUTPUT           = " << !!(_cap.capabilities & V4L2_CAP_VBI_OUTPUT) << std::endl;
   std::cout << "V4L2_CAP_SLICED_VBI_CAPTURE   = " << !!(_cap.capabilities & V4L2_CAP_SLICED_VBI_CAPTURE) << std::endl;
   std::cout << "V4L2_CAP_SLICED_VBI_OUTPUT    = " << !!(_cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) << std::endl;
   std::cout << "V4L2_CAP_RDS_CAPTURE          = " << !!(_cap.capabilities & V4L2_CAP_SLICED_VBI_OUTPUT) << std::endl;
   #ifdef V4L2_CAP_VIDEO_OUTPUT_OVERLAY
   std::cout << "V4L2_CAP_VIDEO_OUTPUT_OVERLAY = " << !!(_cap.capabilities & V4L2_CAP_VIDEO_OUTPUT_OVERLAY) << std::endl;
   #endif
   std::cout << "V4L2_CAP_TUNER                = " << !!(_cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) << std::endl;
   std::cout << "V4L2_CAP_AUDIO                = " << !!(_cap.capabilities & V4L2_CAP_TUNER) << std::endl;
   std::cout << "V4L2_CAP_RADIO                = " << !!(_cap.capabilities & V4L2_CAP_RADIO) << std::endl;
   std::cout << "V4L2_CAP_READWRITE            = " << !!(_cap.capabilities & V4L2_CAP_READWRITE) << std::endl;
   std::cout << "V4L2_CAP_ASYNCIO              = " << !!(_cap.capabilities & V4L2_CAP_ASYNCIO) << std::endl;
   std::cout << "V4L2_CAP_STREAMING            = " << !!(_cap.capabilities & V4L2_CAP_STREAMING) << std::endl;

   _format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
   //std::cout << "Getting Format (VIDIOC_G_FMT)\n";
   if(0 != ioctl(_fd, VIDIOC_G_FMT, &_format))
      throw VideoSourceV4L2::Exception("VIDIOC_G_FMT");
   */
   /*
   std::cout << "size: " << _format.fmt.pix.width  << "x" <<   _format.fmt.pix.height << std::endl;
   //std::cout << "format: " << unfourcc(_format.fmt.pix.pixelformat) << std::endl;
   std::cout << "field flag: " << _format.fmt.pix.field << std::endl;
   std::cout << "bytes per line: " << _format.fmt.pix.bytesperline << std::endl;
   std::cout << "image size: " << _format.fmt.pix.sizeimage << std::endl;
   std::cout << "colourspace: " << _format.fmt.pix.colorspace << std::endl;

   std::cout << "Setting Format (VIDIOC_S_FMT)\n";
   std::cout << "size: " << _format.fmt.pix.width  << "x" <<   _format.fmt.pix.height << std::endl;
   //std::cout << "format: " << unfourcc(_format.fmt.pix.pixelformat) << std::endl;
   std::cout << "field flag: " << _format.fmt.pix.field << std::endl;
   std::cout << "bytes per line: " << _format.fmt.pix.bytesperline << std::endl;
   std::cout << "image size: " << _format.fmt.pix.sizeimage << std::endl;
   std::cout << "colourspace: " << _format.fmt.pix.colorspace << std::endl;
   */
   /*
   if(0 != ioctl(_fd, VIDIOC_S_FMT, &_format))
      throw VideoSourceV4L2::Exception("VIDIOC_S_FMT");
   //std::cout << "Getting Format (VIDIOC_G_FMT)\n";
   if(0 != ioctl(_fd, VIDIOC_G_FMT, &_format))
      throw VideoSourceV4L2::Exception("VIDIOC_G_FMT");
	*/
   /*
   std::cout << "size: " << _format.fmt.pix.width  << "x" <<   _format.fmt.pix.height << std::endl;
   //std::cout << "format: " << unfourcc(_format.fmt.pix.pixelformat) << std::endl;
   std::cout << "field flag: " << _format.fmt.pix.field << std::endl;
   std::cout << "bytes per line: " << _format.fmt.pix.bytesperline << std::endl;
   std::cout << "image size: " << _format.fmt.pix.sizeimage << std::endl;
   std::cout << "colourspace: " << _format.fmt.pix.colorspace << std::endl;
   */
/*
   memset(&_reqbufs, 0, sizeof(_reqbufs));
   _reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
   _reqbufs.memory = V4L2_MEMORY_MMAP;
   _reqbufs.count = requestedNumBuffers;
   _numBuffers = _reqbufs.count;
   //std::cout << "Issuing VIDIOC_REQBUFS ioctl." << std::endl;
   int ret = ioctl(_fd, VIDIOC_REQBUFS, &_reqbufs);
   //std::cout << "Return code: " << ret << std::endl;
   //std::cout << "Number of buffers: " << _reqbufs.count << std::endl;

   _buffers = (FrameBuffer*) calloc(_reqbufs.count, sizeof(FrameBuffer));
   //struct v4l2_buffer* refbuf;
   for(size_t i = 0; i < _reqbufs.count; ++i)
   {
      struct v4l2_buffer buffer;
      memset(&buffer, 0, sizeof(buffer));
      buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buffer.memory = V4L2_MEMORY_MMAP;
      buffer.index = i;
      if(0 != ioctl(_fd, VIDIOC_QUERYBUF, &buffer)) throw VideoSourceV4L2::Exception("VIDIOC_QUERYBUF");
      //if(i == 0) refbuf = &buffer;
      _buffers[i].length = buffer.length;
      _buffers[i].data = mmap(NULL, buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, _fd, buffer.m.offset);
      if(_buffers[i].data == MAP_FAILED) throw VideoSourceV4L2::Exception("mmap");
      if(0 != ioctl(_fd, VIDIOC_QBUF, &buffer)) throw VideoSourceV4L2::Exception("VIDIOC_QBUF");
   }

   //initialise _frameSize
   _frameSize = Eigen::Vector2i(_format.fmt.pix.width, _format.fmt.pix.height);
   //start streaming
   if(0 != ioctl(_fd, VIDIOC_STREAMON, &_reqbufs.type)) throw VideoSourceV4L2::Exception("STREAMON");
   */
}

VideoSourceV4L2::~VideoSourceV4L2()
{
	/*
   for(int i = 0; i < _reqbufs.count; i++)
      munmap(_buffers[i].data, _buffers[i].length);
	  */
   if(_fd) close(_fd);
}
const ImageRegionConstRGB VideoSourceV4L2::getNextFrame()
{
	/*
   struct v4l2_buffer buffer;
   buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
   buffer.memory = V4L2_MEMORY_MMAP;
   int err = ioctl(_fd, VIDIOC_DQBUF, &buffer);
   //std::cout << "buffer.userptr = " << buffer.userptr << std::endl;
   while(err != 0 && errno == EAGAIN)
   {
      usleep(10);
      err = ioctl(_fd, VIDIOC_DQBUF, &buffer);
   }
   if(err != 0)
   {
      std::stringstream ss;
      ss << "VIDIOC_DQBUF: ";
      if(errno == EIO) ss << "EIO";
      if(errno == EAGAIN) ss << "EAGAIN";
      if(errno == EINVAL) ss << "EINVAL";
      if(errno == ENOMEM) ss << "ENOMEM";
      throw VideoSourceV4L2::Exception(ss.str());
   }

   int id = buffer.index;
   unsigned char* data = static_cast<unsigned char*>(_buffers[id].data);
   timeval timestamp = buffer.timestamp;
*/
	/*
   int w = _format.fmt.pix.width;
   int h = _format.fmt.pix.height;
   unsigned char* p = data;
   
   _frame = ImageRGB(w, h);
   for(int y = 0; y < h; ++y)
   {
      for(int x = 0; x < w; ++x)
      {
         unsigned char y1 = *(p++);
         unsigned char u = *(p++);
         unsigned char y2 = *(p++);
         unsigned char v = *(p++);
         unsigned char r, g, b;
         yuv444_to_rgb888(y1, u, v, &r, &g, &b);
         _frame[y][x] = btl::PixelRGB(r, g, b);
         ++x;
         yuv444_to_rgb888(y2, u, v, &r, &g, &b);
         _frame[y][x] = btl::PixelRGB(r, g, b);
      }
   }
   err = ioctl(_fd, VIDIOC_QBUF, &buffer);
   return ImageRegionConstRGB(_frame);
   */
   return ImageRegionConstRGB();
}

void VideoSourceV4L2::yuv444_to_rgb888(
   unsigned char y,
   unsigned char u,
   unsigned char v,
   unsigned char* r,
   unsigned char* g,
   unsigned char* b
)
{
   /*
   u -= 128;
   v -= 128;
   *r = y + v + (v >> 2) + (v >> 3) + (v >> 5);
   *g = y - ((u >> 2) + (u >> 4) + (u >> 5)) - ((v >> 1) + (v >> 3) + (v >> 4) + (v >> 5));
   *b = y + u + (u >> 1) + (u >> 2) + (u >> 6);
   // */
   //  /*
   int rt = (int)((double)y + 1.402 * (double)(v - 128));
   int gt = (int)((double)y - 0.3444 * (double)(u - 128) - 0.714 * (double)(v - 128));
   int bt = (int)((double)y + 1.772 * (double)(u - 128));
   if(rt > 255) *r = 255;
   else if(rt < 0) *r = 0;
   else *r = (unsigned char) rt;
   if(gt > 255) *g = 255;
   else if(gt < 0) *g = 0;
   else *g = (unsigned char) gt;
   if(bt > 255) *b = 255;
   else if(bt < 0) *b = 0;
   else *b = (unsigned char) bt;
   // */
   /*
   unsigned char c = y - 16, d = u - 128, e = v - 128;
   *r = (298 * c + 409 * e + 128) >> 8;
   *g = (298 * c - 100 * d - 208 * e + 128) >> 8;
   *b = (298 * c + 516 * d + 128) >> 8;
   */
}

} //namespace videosource
} //namespace extra
} //namespace btl
